
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_mission.c
  * @author     baiyang
  * @date       2022-5-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_mission.h"

#include <float.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// 
bool mission_stored_in_location(uint16_t id)
{
    switch (id) {
    case MAV_CMD_NAV_WAYPOINT:
    case MAV_CMD_NAV_LOITER_UNLIM:
    case MAV_CMD_NAV_LOITER_TURNS:
    case MAV_CMD_NAV_LOITER_TIME:
    case MAV_CMD_NAV_LAND:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
    case MAV_CMD_NAV_LOITER_TO_ALT:
    case MAV_CMD_NAV_SPLINE_WAYPOINT:
    case MAV_CMD_NAV_GUIDED_ENABLE:
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_LAND_START:
    case MAV_CMD_DO_GO_AROUND:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_PAYLOAD_PLACE:
        return true;
    default:
        return false;
    }
}

MAV_MISSION_RESULT mission_SanityCheckParams(const mavlink_mission_item_int_t* packet)
{
    uint8_t nan_mask = 0;
    switch (packet->command) {
    case MAV_CMD_NAV_WAYPOINT:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_LAND:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_TAKEOFF:
        nan_mask = ~(1 << 3); // param 4 can be nan
        break;
    case MAV_CMD_NAV_VTOL_LAND:
        nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan
        break;
    default:
        nan_mask = 0xff;
        break;
    }

    if (((nan_mask & (1 << 0)) && isnan(packet->param1)) ||
        isinf(packet->param1)) {
        return MAV_MISSION_INVALID_PARAM1;
    }
    if (((nan_mask & (1 << 1)) && isnan(packet->param2)) ||
        isinf(packet->param2)) {
        return MAV_MISSION_INVALID_PARAM2;
    }
    if (((nan_mask & (1 << 2)) && isnan(packet->param3)) ||
        isinf(packet->param3)) {
        return MAV_MISSION_INVALID_PARAM3;
    }
    if (((nan_mask & (1 << 3)) && isnan(packet->param4)) ||
        isinf(packet->param4)) {
        return MAV_MISSION_INVALID_PARAM4;
    }
    return MAV_MISSION_ACCEPTED;
}

// mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
//  return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_MavlinkIntToMissionCmd(const mavlink_mission_item_int_t* packet, Mission_Command* cmd)
{
    // command's position in mission list and mavlink id
    cmd->index = packet->seq;
    cmd->id = packet->command;
    memset(&(cmd->content.location),0,sizeof(cmd->content.location));

    MAV_MISSION_RESULT param_check = mission_SanityCheckParams(packet);
    if (param_check != MAV_MISSION_ACCEPTED) {
        return param_check;
    }

    // command specific conversions from mavlink packet to mission command
    switch (cmd->id) {

    case 0:
        // this is reserved for storing 16 bit command IDs
        return MAV_MISSION_INVALID;

    case MAV_CMD_NAV_WAYPOINT: {                        // MAV ID: 16
        /*
          the 15 byte limit means we can't fit both delay and radius
          in the cmd structure. When we expand the mission structure
          we can do this properly
         */
        // delay at waypoint in seconds (this is for copters???)
        cmd->p1 = packet->param1;
    }
    break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        cmd->p1 = fabsf(packet->param3);                  // store radius as 16bit since no other params are competing for space
        cmd->content.location.tflags.loiter_ccw = (packet->param3 < 0);    // -1 = counter clockwise, +1 = clockwise
        break;

    case MAV_CMD_NAV_LOITER_TURNS: {                    // MAV ID: 18
        uint16_t num_turns = packet->param1;              // param 1 is number of times to circle is held in low p1
        uint16_t radius_m = fabsf(packet->param3);        // param 3 is radius in meters is held in high p1
        cmd->p1 = (radius_m<<8) | (num_turns & 0x00FF);   // store radius in high byte of p1, num turns in low byte of p1
        cmd->content.location.tflags.loiter_ccw = (packet->param3 < 0);
        cmd->content.location.tflags.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
    }
    break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        cmd->p1 = packet->param1;                         // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius.
        cmd->content.location.tflags.loiter_ccw = (packet->param3 < 0);
        cmd->content.location.tflags.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        cmd->p1 = packet->param1;                         // abort target altitude(m)  (plane only)
        cmd->content.location.tflags.loiter_ccw = math_flt_negative(packet->param4); // yaw direction, (plane deepstall only)
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        cmd->p1 = packet->param1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        cmd->p1 = packet->param1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        cmd->p1 = fabsf(packet->param2);                  // param2 is radius in meters
        cmd->content.location.tflags.loiter_ccw = (packet->param2 < 0);
        cmd->content.location.tflags.loiter_xtrack = (packet->param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
        cmd->p1 = packet->param1;                         // delay at waypoint in seconds
        break;

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        cmd->p1 = packet->param1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        cmd->content.nav_delay.seconds = packet->param1; // delay in seconds
        cmd->content.nav_delay.hour_utc = packet->param2;// absolute time's hour (utc)
        cmd->content.nav_delay.min_utc = packet->param3;// absolute time's min (utc)
        cmd->content.nav_delay.sec_utc = packet->param4; // absolute time's second (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        cmd->content.delay.seconds = packet->param1;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        cmd->content.distance.meters = packet->param1;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        cmd->content.yaw.angle_deg = packet->param1;      // target angle in degrees
        cmd->content.yaw.turn_rate_dps = packet->param2;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        cmd->content.yaw.direction = packet->param3;      // -1 = ccw, +1 = cw
        cmd->content.yaw.relative_angle = packet->param4; // lng=0: absolute angle provided, lng=1: relative angle provided
        break;

    case MAV_CMD_DO_SET_MODE:                           // MAV ID: 176
        cmd->p1 = packet->param1;                         // flight mode identifier
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
        cmd->content.jump.target = packet->param1;        // jump-to command number
        cmd->content.jump.num_times = packet->param2;     // repeat count
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        cmd->content.speed.speed_type = packet->param1;   // 0 = airspeed, 1 = ground speed
        cmd->content.speed.target_ms = packet->param2;    // target speed in m/s
        cmd->content.speed.throttle_pct = packet->param3; // throttle as a percentage from 0 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:
        cmd->p1 = packet->param1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_LAND_START:                         // MAV ID: 189
        break;

    case MAV_CMD_DO_GO_AROUND:                          // MAV ID: 191
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        cmd->p1 = packet->param1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        cmd->p1 = packet->param1;                         // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_PARACHUTE:                         // MAV ID: 208
        cmd->p1 = packet->param1;                        // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        cmd->p1 = packet->param1;                         // normal=0 inverted=1
        break;

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        cmd->p1 = packet->param1;                         // max time in seconds the external controller will be allowed to control the vehicle
        cmd->content.guided_limits.alt_min = packet->param2;  // min alt below which the command will be aborted.  0 for no lower alt limit
        cmd->content.guided_limits.alt_max = packet->param3;  // max alt above which the command will be aborted.  0 for no upper alt limit
        cmd->content.guided_limits.horiz_max = packet->param4;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_NAV_VTOL_TAKEOFF:
        break;

    case MAV_CMD_NAV_VTOL_LAND:
        break;

    case MAV_CMD_NAV_SET_YAW_SPEED:
        cmd->content.set_yaw_speed.angle_deg = packet->param1;        // target angle in degrees
        cmd->content.set_yaw_speed.speed = packet->param2;            // speed in meters/second
        cmd->content.set_yaw_speed.relative_angle = packet->param3;   // 0 = absolute angle, 1 = relative angle
        break;
    default:
        // unrecognised command
        return MAV_MISSION_UNSUPPORTED;
    }

    // copy location from mavlink to command
    if (mission_stored_in_location(cmd->id)) {

        // sanity check location
        if (!location_check_lat(packet->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(packet->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        if (isnan(packet->z) || fabsf(packet->z) >= LOCATION_ALT_MAX_M) {
            return MAV_MISSION_INVALID_PARAM7;
        }

        cmd->content.location.lat = packet->x;
        cmd->content.location.lng = packet->y;

        cmd->content.location.alt = packet->z * 100.0f;       // convert packet's alt (m) to cmd alt (cm)

        switch (packet->frame) {

        case MAV_FRAME_MISSION:
        case MAV_FRAME_GLOBAL:
        case MAV_FRAME_GLOBAL_INT:
            cmd->content.location.tflags.relative_alt = 0;
            break;

        case MAV_FRAME_GLOBAL_RELATIVE_ALT:
        case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
            cmd->content.location.tflags.relative_alt = 1;
            break;
        default:
            return MAV_MISSION_UNSUPPORTED_FRAME;
        }
    }

    // if we got this far then it must have been successful
    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t *packet,
        mavlink_mission_item_int_t *mav_cmd)
{
    // TODO: rename mav_cmd to mission_item_int
    // TODO: rename packet to mission_item
    mav_cmd->param1 = packet->param1;
    mav_cmd->param2 = packet->param2;
    mav_cmd->param3 = packet->param3;
    mav_cmd->param4 = packet->param4;
    mav_cmd->z = packet->z;
    mav_cmd->seq = packet->seq;
    mav_cmd->command = packet->command;
    mav_cmd->target_system = packet->target_system;
    mav_cmd->target_component = packet->target_component;
    mav_cmd->frame = packet->frame;
    mav_cmd->current = packet->current;
    mav_cmd->autocontinue = packet->autocontinue;

    /*
      the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT
      is to pass the lat/lng in MISSION_ITEM_INT straight through, and
      for MISSION_ITEM multiply by 1e7 here. We need an exception for
      any commands which use the x and y fields not as
      latitude/longitude.
     */
    switch (packet->command) {
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        mav_cmd->x = packet->x;
        mav_cmd->y = packet->y;
        break;

    default:
        // all other commands use x and y as lat/lon. We need to
        // multiply by 1e7 to convert to int32_t
        if (!location_check_lat(packet->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(packet->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        mav_cmd->x = packet->x * 1.0e7f;
        mav_cmd->y = packet->y * 1.0e7f;
        break;
    }

    return MAV_MISSION_ACCEPTED;
}

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t *item_int,
        mavlink_mission_item_t *item)
{
    item->param1 = item_int->param1;
    item->param2 = item_int->param2;
    item->param3 = item_int->param3;
    item->param4 = item_int->param4;
    item->z = item_int->z;
    item->seq = item_int->seq;
    item->command = item_int->command;
    item->target_system = item_int->target_system;
    item->target_component = item_int->target_component;
    item->frame = item_int->frame;
    item->current = item_int->current;
    item->autocontinue = item_int->autocontinue;

    switch (item_int->command) {
    case MAV_CMD_DO_DIGICAM_CONTROL:
    case MAV_CMD_DO_DIGICAM_CONFIGURE:
        item->x = item_int->x;
        item->y = item_int->y;
        break;

    default:
        // all other commands use x and y as lat/lon. We need to
        // multiply by 1e-7 to convert to float
        item->x = item_int->x * 1.0e-7f;
        item->y = item_int->y * 1.0e-7f;
        if (!location_check_lat(item->x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!location_check_lng(item->y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        break;
    }

    return MAV_MISSION_ACCEPTED;
}

// mavlink_cmd_long_to_mission_cmd - converts a mavlink cmd long to an AP_Mission::Mission_Command object which can be stored to eeprom
// return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_MavlinkCmdLongToMissionCmd(const mavlink_command_long_t* packet, Mission_Command* cmd)
{
    mavlink_mission_item_int_t miss_item = {0};

    miss_item.param1 = packet->param1;
    miss_item.param2 = packet->param2;
    miss_item.param3 = packet->param3;
    miss_item.param4 = packet->param4;

    miss_item.command = packet->command;
    miss_item.target_system = packet->target_system;
    miss_item.target_component = packet->target_component;

    return mission_MavlinkIntToMissionCmd(&miss_item, cmd);
}

// mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
//  return true on success, false on failure
bool mission_MissionCmdToMavlinkInt(const Mission_Command* cmd, mavlink_mission_item_int_t* packet)
{
    // command's position in mission list and mavlink id
    packet->seq = cmd->index;
    packet->command = cmd->id;

    // set defaults
    packet->current = 0;     // 1 if we are passing back the mission command that is currently being executed
    packet->param1 = 0;
    packet->param2 = 0;
    packet->param3 = 0;
    packet->param4 = 0;
    packet->autocontinue = 1;

    // command specific conversions from mission command to mavlink packet
    switch (cmd->id) {
    case 0:
        // this is reserved for 16 bit command IDs
        return false;

    case MAV_CMD_NAV_WAYPOINT:                          // MAV ID: 16
        // delay at waypoint in seconds
        packet->param1 = cmd->p1;
        break;

    case MAV_CMD_NAV_LOITER_UNLIM:                      // MAV ID: 17
        packet->param3 = (float)cmd->p1;
        if (cmd->content.location.tflags.loiter_ccw) {
            packet->param3 *= -1;
        }
        break;

    case MAV_CMD_NAV_LOITER_TURNS:                      // MAV ID: 18
        packet->param1 = LOWBYTE(cmd->p1);                // number of times to circle is held in low byte of p1
        packet->param3 = HIGHBYTE(cmd->p1);               // radius is held in high byte of p1
        if (cmd->content.location.tflags.loiter_ccw) {
            packet->param3 = -packet->param3;
        }
        packet->param4 = cmd->content.location.tflags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_LOITER_TIME:                       // MAV ID: 19
        packet->param1 = cmd->p1;                         // loiter time in seconds
        if (cmd->content.location.tflags.loiter_ccw) {
            packet->param3 = -1;
        } else {
            packet->param3 = 1;
        }
        packet->param4 = cmd->content.location.tflags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:                  // MAV ID: 20
        break;

    case MAV_CMD_NAV_LAND:                              // MAV ID: 21
        packet->param1 = cmd->p1;                        // abort target altitude(m)  (plane only)
        packet->param4 = cmd->content.location.tflags.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only)
        break;

    case MAV_CMD_NAV_TAKEOFF:                           // MAV ID: 22
        packet->param1 = cmd->p1;                         // minimum pitch (plane only)
        break;

    case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:           // MAV ID: 30
        packet->param1 = cmd->p1;                         // Climb/Descend
        // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
        // 1 = Climb, cmd complete at or above indicated alt.
        // 2 = Descend, cmd complete at or below indicated alt.
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:                     // MAV ID: 31
        packet->param2 = cmd->p1;                        // loiter radius(m)
        if (cmd->content.location.tflags.loiter_ccw) {
            packet->param2 = -packet->param2;
        }
        packet->param4 = cmd->content.location.tflags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:                   // MAV ID: 82
        packet->param1 = cmd->p1;                         // delay at waypoint in seconds
        break;

    case MAV_CMD_NAV_GUIDED_ENABLE:                     // MAV ID: 92
        packet->param1 = cmd->p1;                         // on/off. >0.5 means "on", hand-over control to external controller
        break;

    case MAV_CMD_NAV_DELAY:                            // MAV ID: 93
        packet->param1 = cmd->content.nav_delay.seconds; // delay in seconds
        packet->param2 = cmd->content.nav_delay.hour_utc; // absolute time's day of week (utc)
        packet->param3 = cmd->content.nav_delay.min_utc; // absolute time's hour (utc)
        packet->param4 = cmd->content.nav_delay.sec_utc; // absolute time's min (utc)
        break;

    case MAV_CMD_CONDITION_DELAY:                       // MAV ID: 112
        packet->param1 = cmd->content.delay.seconds;      // delay in seconds
        break;

    case MAV_CMD_CONDITION_DISTANCE:                    // MAV ID: 114
        packet->param1 = cmd->content.distance.meters;    // distance in meters from next waypoint
        break;

    case MAV_CMD_CONDITION_YAW:                         // MAV ID: 115
        packet->param1 = cmd->content.yaw.angle_deg;      // target angle in degrees
        packet->param2 = cmd->content.yaw.turn_rate_dps;  // 0 = use default turn rate otherwise specific turn rate in deg/sec
        packet->param3 = cmd->content.yaw.direction;      // -1 = ccw, +1 = cw
        packet->param4 = cmd->content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
        break;

    case MAV_CMD_DO_SET_MODE:                           // MAV ID: 176
        packet->param1 = cmd->p1;                         // set flight mode identifier
        break;

    case MAV_CMD_DO_JUMP:                               // MAV ID: 177
        packet->param1 = cmd->content.jump.target;        // jump-to command number
        packet->param2 = cmd->content.jump.num_times;     // repeat count
        break;

    case MAV_CMD_DO_CHANGE_SPEED:                       // MAV ID: 178
        packet->param1 = cmd->content.speed.speed_type;   // 0 = airspeed, 1 = ground speed
        packet->param2 = cmd->content.speed.target_ms;    // speed in m/s
        packet->param3 = cmd->content.speed.throttle_pct; // throttle as a percentage from 0 ~ 100%
        break;

    case MAV_CMD_DO_SET_HOME:                           // MAV ID: 179
        packet->param1 = cmd->p1;                         // p1=0 means use current location, p=1 means use provided location
        break;

    case MAV_CMD_DO_SET_ROI:                            // MAV ID: 201
        packet->param1 = cmd->p1;                         // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
        break;

    case MAV_CMD_DO_FENCE_ENABLE:                       // MAV ID: 207
        packet->param1 = cmd->p1;                         // action 0=disable, 1=enable
        break;

    case MAV_CMD_DO_PARACHUTE:                          // MAV ID: 208
        packet->param1 = cmd->p1;                         // action 0=disable, 1=enable, 2=release.  See PARACHUTE_ACTION enum
        break;

    case MAV_CMD_DO_INVERTED_FLIGHT:                    // MAV ID: 210
        packet->param1 = cmd->p1;                         // normal=0 inverted=1
        break;

    case MAV_CMD_DO_GUIDED_LIMITS:                      // MAV ID: 222
        packet->param1 = cmd->p1;                         // max time in seconds the external controller will be allowed to control the vehicle
        packet->param2 = cmd->content.guided_limits.alt_min;  // min alt below which the command will be aborted.  0 for no lower alt limit
        packet->param3 = cmd->content.guided_limits.alt_max;  // max alt above which the command will be aborted.  0 for no upper alt limit
        packet->param4 = cmd->content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
        break;

    case MAV_CMD_DO_SET_REVERSE:
        packet->param1 = cmd->p1;   // 0 = forward, 1 = reverse
        break;

    case MAV_CMD_DO_ENGINE_CONTROL:
        packet->param1 = cmd->content.do_engine_control.start_control?1:0;
        packet->param2 = cmd->content.do_engine_control.cold_start?1:0;
        packet->param3 = cmd->content.do_engine_control.height_delay_cm*0.01f;
        break;

    case MAV_CMD_NAV_PAYLOAD_PLACE:
        packet->param1 = cmd->p1*.001f; // copy max-descend parameter (m->cm)
        break;

    case MAV_CMD_NAV_SET_YAW_SPEED:
        packet->param1 = cmd->content.set_yaw_speed.angle_deg;        // target angle in degrees
        packet->param2 = cmd->content.set_yaw_speed.speed;            // speed in meters/second
        packet->param3 = cmd->content.set_yaw_speed.relative_angle;   // 0 = absolute angle, 1 = relative angle
        break;
    default:
        // unrecognised command
        return false;
    }

    // copy location from mavlink to command
    if (mission_stored_in_location(cmd->id)) {
        packet->x = cmd->content.location.lat;
        packet->y = cmd->content.location.lng;

        packet->z = cmd->content.location.alt * 0.01f;   // cmd alt in cm to m
        if (cmd->content.location.tflags.relative_alt) {
            packet->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
        } else {
            packet->frame = MAV_FRAME_GLOBAL;
        }
        // don't ever return terrain mission items if no terrain support
        if (cmd->content.location.tflags.terrain_alt) {
            return false;
       }
    }

    // if we got this far then it must have been successful
    return true;
}

/*------------------------------------test------------------------------------*/


